package com.grt192.mechanism;

import com.grt192.core.Mechanism;
import com.iaroc.irobot.IRobotCreate;

/**
 * A CreateBase manages IRobotCreate drive function
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class CreateBase extends Mechanism {

    public static final int MAX_VELOCITY = 500;
    private final IRobotCreate create;

    public CreateBase(IRobotCreate create) {
        this.create = create;
    }

    /**
     * Drive by commanding each wheel's motorspeed
     * @param left speed of left wheel in mm/s [-500...500]
     * @param right speed of right wheel in mm/s [-500...500]
     */
    public void velocityDrive(int left, int right) {
        if (create != null) {
            int x = limit(-MAX_VELOCITY, MAX_VELOCITY, left);
            int y = limit(-MAX_VELOCITY, MAX_VELOCITY, right);
            log("Drive:" + x + "\t" + y);
            create.driveDirect(
                    x,
                    y);
        }
    }

    /**
     * Drive by commanding each wheel's percent of maximum speed
     * @param left % of max velocity [-1...1]
     * @param right % of max velocity [-1...1]
     */
    public void percentDrive(double left, double right) {
        velocityDrive(
                (int) (left * MAX_VELOCITY),
                (int) (right * MAX_VELOCITY));
    }

    
    private static int limit(int min, int max, int in) {
        if (in < min) {
            return min;
        }
        if (in > max) {
            return max;
        }
        return in;

    }
}
